///*
// * imu660.h
// *
// *  Created on: 2024年6月3日
// *      Author: Kenobi
// */
//
#ifndef IMU660_H_
#define IMU660_H_
//
#include "zf_common_headfile.h"
//
//#define G           9.80665f                // m/s^2
//#define RadtoDeg    57.324841f              //弧度到角度 (弧度 * 180/3.1415)
//#define DegtoRad    0.0174533f              //角度到弧度 (角度 * 3.1415/180)
//
////三轴浮点型
//typedef struct
//{
//    int16_t X;
//    int16_t Y;
//    int16_t Z;
//}FLOAT_XYZ;
//
////姿态解算后的角度
//typedef struct
//{
//    float rol;
//    float pit;
//    float yaw;
//}FLOAT_ANGLE;
//
//extern FLOAT_XYZ    Acc,Acc_out,Gyro;                 //把陀螺仪的各通道读出的数据，转换成弧度制
//extern FLOAT_ANGLE Att_Angle;
//void Prepare_Data(FLOAT_XYZ *acc_in,FLOAT_XYZ *acc_out);
//void IMUupdate(FLOAT_XYZ *Gyr_rad,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle);
//

#define ANGLE_MAX       180.0f
#define ANGLE_MIN       -180.0f

#define LIMIT( x,min,max )  ( (x) < (min)  ? (min) : ( (x) > (max) ? (max) : (x) ))
#define ABS(x)                          ((x)>0?(x):-(x))
#define DIV(Number, Prescaler, Threshold) ((Prescaler == 0.0f)? Threshold: (Number/Prescaler))
#define Power2(x)                   (x * x)
#define MAX_2(x,y)                  ((x >= y)? x: y)
#define MAX_3(x,y,z)                ((z >= MAX_2(x,y))? z: MAX_2(x,y))

float translateAngle(float angle);
float arctan1(float tan);
float arctan2(float x, float y);
float arcsin(float i);

#ifndef u8
#define u8 unsigned char
#endif

#ifndef s8
#define s8 char
#endif

#ifndef XYZ_Data
#define XYZ_Data
typedef struct XYZ_Data_f
{
    float x;
    float y;
    float z;
}XYZ_Data_f;

typedef struct XYZ_Data_s32
{
    long x;
    long y;
    long z;
}XYZ_Data_s32;

typedef struct XYZ_Data_s16
{
    short x;
    short y;
    short z;
}XYZ_Data_s16;

typedef struct XYZ_Data_s8
{
    s8 x;
    s8 y;
    s8 z;
}XYZ_Data_s8;
#endif
typedef struct Pose_Flag
{
    u8 run;
    u8 use_mag;
}Pose_Flag;

typedef struct Pose_DInterface
{
    float *a_x;
    float *a_y;
    float *a_z;

    float *g_x;
    float *g_y;
    float *g_z;

    float *m_x;
    float *m_y;
    float *m_z;
}Pose_DInterface;

typedef struct Pose_Interface
{
    Pose_DInterface data;
}Pose_Interface;

typedef struct Pose_Data
{
    float yaw;
    float rol;
    float pit;

    float rotate_matrix[3][3]; //旋转矩阵

    XYZ_Data_f acc_world;    //世界坐标系下的加速度
    XYZ_Data_f mag_world;    //世界坐标系下的磁场强度 -- 只与无人机位置有关的量

    XYZ_Data_f acc_correct;   //机体坐标系下的加速度 -- 矫正了俯仰翻滚后的加速度
    XYZ_Data_f mag_correct;   //机体坐标系下的磁场强度 -- 矫正了俯仰翻滚后的磁场强度
    XYZ_Data_f gyro_correct;  //融合加速度和磁力计数据，矫正后的陀螺仪值
}Pose_Data;

typedef struct Pose_Process
{
    float mag_yaw;       //磁力计的偏航角
    float mag_yaw_bias;     //磁力计矫正的偏航角偏差
    float quaternion[4];    //四元数
    XYZ_Data_f error;      //由加速度计与等效重力的偏差
    XYZ_Data_f error_integral; //误差积分
}Pose_Process;

typedef struct Pose_Parameter
{
    float correct_kp;   //矫正速度
    float error_kp;     //矫正速度
    float error_ki;
}Pose_Parameter;

typedef struct Pose_Module
{
    Pose_Flag flag;
    Pose_Interface interface;
    Pose_Process process;
    Pose_Data data;
    Pose_Parameter parameter;
}Pose_Module;

//初始化结构体
void initPose_Module(Pose_Module *pose);
//计算姿态
void calculatePose_Module(Pose_Module *pose, float cycle);

#endif /* IMU660_H_ */
